﻿/////////////////////////////////////////////////////////////////
//
// Silverlight 5: 3D Physics using Balder and Jiglibx
// by Andy Beaulieu - http://www.andybeaulieu.com
//
// LICENSE: This code is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
// without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. ANY 
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS
// OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
/////////////////////////////////////////////////////////////////
using System;
using System.Net;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Documents;
using System.Windows.Ink;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Animation;
using System.Windows.Shapes;
using Balder.Objects.Geometries;
using Microsoft.Xna.Framework;
using JigLibX.Physics;
using JigLibX.Collision;
using jlxg = JigLibX.Geometry;
using JigLibX.Geometry;
using Balder;
using System.Collections.Generic;

namespace Spritehand.Physics3D
{
    public class PhysicsRagDoll : IPhysicsObject
    {
        PhysicsSystem _world;

        public PhysicsRagDoll()
        {
          
        }

        public enum RagdollType
        {
            Simple, Complex
        }

        private enum LimbId
        {
            Torso,
            Head,
            UpperLegLeft,
            UpperLegRight,
            LowerLegLeft,
            LowerLegRight,
            UpperArmLeft,
            UpperArmRight,
            LowerArmLeft,
            LowerArmRight,
            FootLeft,
            FootRight,
            HandLeft,
            HandRight,
            Hips,
            NumLimbs
        }

        private enum JointId
        {
            Neck,
            ShoulderLeft,
            ShoulderRight,
            ElbowLeft,
            ElbowRight,
            HipLeft,
            HipRight,
            KneeLeft,
            KneeRight,
            WristLeft,
            WristRight,
            AnkleLeft,
            AnkleRight,
            Spine,
            NumJoints
        }

        IPhysicsObject[] limbs;
        HingeJoint[] joints;

        int numLimbs;
        int numJoints;

        private void DisableCollisions(Body rb0, Body rb1)
        {
            if ((rb0.CollisionSkin == null) || (rb1.CollisionSkin == null))
                return;
            rb0.CollisionSkin.NonCollidables.Add(rb1.CollisionSkin);
            rb1.CollisionSkin.NonCollidables.Add(rb0.CollisionSkin);
        }

        private Body _physicsBody;
        public Body PhysicsBody()
        {
            return _physicsBody;
        }

        public void Draw()
        {

            foreach (IPhysicsObject limb in limbs)
            {
                if (limb != null)
                {
                    limb.Draw();
                }
            }

       


        }



        public PhysicsRagDoll(PhysicsSystem world, RagdollType type, float density, Container container, List<IPhysicsObject> physicsObjects, Balder.Math.Coordinate position )
        {
            if (type == RagdollType.Complex)
            {
                numLimbs = (int)LimbId.NumLimbs;
                numJoints = (int)JointId.NumJoints;
            }
            else
            {
                numLimbs = (int)LimbId.NumLimbs - 5;
                numJoints = (int)JointId.NumJoints - 5;
            }

            limbs = new IPhysicsObject[numLimbs];
            joints = new HingeJoint[numJoints];

           
            limbs[(int)LimbId.Head] = new PhysicsSphere(_world, 0.15f, Balder.Math.Matrix.Identity, new Balder.Math.Coordinate(0,0,0));
            limbs[(int)LimbId.UpperLegLeft] = new PhysicsCapsule(_world, 0.08f, 0.3f, new Balder.Math.Coordinate(90,0,0), new Balder.Math.Coordinate(0, 0, 0));
            limbs[(int)LimbId.UpperLegRight] = new PhysicsCapsule(_world, 0.08f, 0.3f,  new Balder.Math.Coordinate(90,0,0), new Balder.Math.Coordinate(0,0,0));
            limbs[(int)LimbId.LowerLegLeft] = new PhysicsCapsule(_world, 0.08f, 0.3f,  new Balder.Math.Coordinate(90,0,0), new Balder.Math.Coordinate(0,0,0));
            limbs[(int)LimbId.LowerLegRight] = new PhysicsCapsule(_world, 0.08f, 0.3f,  new Balder.Math.Coordinate(90,0,0), new Balder.Math.Coordinate(0,0,0));
            limbs[(int)LimbId.UpperArmLeft] = new PhysicsCapsule(_world, 0.07f, 0.2f,  new Balder.Math.Coordinate(0,0,90), new Balder.Math.Coordinate(0,0,0));
            limbs[(int)LimbId.UpperArmRight] = new PhysicsCapsule(_world, 0.07f, 0.2f,  new Balder.Math.Coordinate(0,0,90), new Balder.Math.Coordinate(0,0,0));
            limbs[(int)LimbId.LowerArmLeft] = new PhysicsCapsule(_world, 0.06f, 0.2f,  new Balder.Math.Coordinate(0,0,90), new Balder.Math.Coordinate(0,0,0));
            limbs[(int)LimbId.LowerArmRight] = new PhysicsCapsule(_world, 0.06f, 0.2f,  new Balder.Math.Coordinate(0,0,90), new Balder.Math.Coordinate(0,0,0));

            if (type == RagdollType.Complex)
            {
                limbs[(int)LimbId.FootLeft] = new PhysicsSphere(_world, 0.07f,  Balder.Math.Matrix.Identity, new Balder.Math.Coordinate(0,0,0));
                limbs[(int)LimbId.FootRight] = new PhysicsSphere(_world, 0.07f,  Balder.Math.Matrix.Identity, new Balder.Math.Coordinate(0,0,0));
                limbs[(int)LimbId.HandLeft] = new PhysicsSphere(_world, 0.05f,  Balder.Math.Matrix.Identity, new Balder.Math.Coordinate(0,0,0));
                limbs[(int)LimbId.HandRight] = new PhysicsSphere(_world, 0.05f,  Balder.Math.Matrix.Identity, new Balder.Math.Coordinate(0,0,0));
                limbs[(int)LimbId.Torso] = new PhysicsBox(_world, new Balder.Math.Coordinate(0.2f, 0.4f, 0.35f), Balder.Math.Matrix.Identity, new Balder.Math.Coordinate(0, 0, 0));
                limbs[(int)LimbId.Hips] = new PhysicsBox(_world, new Balder.Math.Coordinate(0.2f, 0.2f, 0.35f), Balder.Math.Matrix.Identity, new Balder.Math.Coordinate(0, 0, 0));
            }
            else
            {
                limbs[(int)LimbId.Torso] = new PhysicsBox(_world, new Balder.Math.Coordinate(0.2f, 0.6f, 0.35f), Balder.Math.Matrix.Identity, new Balder.Math.Coordinate(0, 0, 0));
            }

            limbs[(int)LimbId.Head].PhysicsBody().Position = new Vector3(0.03f, 0.5f,0);
            limbs[(int)LimbId.UpperLegLeft].PhysicsBody().Position = new Vector3(0, -0.4f, 0.12f);
            limbs[(int)LimbId.UpperLegRight].PhysicsBody().Position = new Vector3(0, -0.4f, -0.12f);
            limbs[(int)LimbId.LowerLegLeft].PhysicsBody().Position = new Vector3(0, -0.7f, 0.12f);
            limbs[(int)LimbId.LowerLegRight].PhysicsBody().Position = new Vector3(0, -0.7f, -0.12f);
            limbs[(int)LimbId.UpperArmLeft].PhysicsBody().Position = new Vector3(0, 0.25f, 0.25f);
            limbs[(int)LimbId.UpperArmRight].PhysicsBody().Position = new Vector3(0, 0.25f, -0.25f);
            limbs[(int)LimbId.LowerArmLeft].PhysicsBody().Position = new Vector3(0, 0.25f, 0.5f);
            limbs[(int)LimbId.LowerArmRight].PhysicsBody().Position = new Vector3(0, 0.25f, -0.5f);

            if (type == RagdollType.Complex)
            {
                limbs[(int)LimbId.FootLeft].PhysicsBody().Position = new Vector3(0.13f, -0.85f, 0.12f);
                limbs[(int)LimbId.FootRight].PhysicsBody().Position = new Vector3(0.13f, -0.85f, -0.12f);
                limbs[(int)LimbId.HandLeft].PhysicsBody().Position = new Vector3(0, 0.25f, 0.72f);
                limbs[(int)LimbId.HandRight].PhysicsBody().Position = new Vector3(0, 0.25f, -0.72f);
                limbs[(int)LimbId.Torso].PhysicsBody().Position = new Vector3(0, 0.2f, 0.0f);
                limbs[(int)LimbId.Hips].PhysicsBody().Position = new Vector3(0, -0.1f, 0.0f);
            }
            else
            {
                limbs[(int)LimbId.Torso].PhysicsBody().Position = new Vector3(0, 0, 0);
            }

            _physicsBody = limbs[(int)LimbId.Torso].PhysicsBody();


            // set up hinge joints
            float haldWidth = 0.2f;
            float sidewaysSlack = 0.1f;
            float damping = 0.5f;

            for (int i = 0; i < numJoints; i++)
                joints[i] = new HingeJoint();

            if (type == RagdollType.Complex)
            {
                joints[(int)JointId.Spine].Initialise(limbs[(int)LimbId.Hips].PhysicsBody(),
                                        limbs[(int)LimbId.Torso].PhysicsBody(),
                                        new Vector3(0.0f, 0.0f, 1.0f),
                                        new Vector3(0.0f, 0.1f, 0.0f),
                                        haldWidth, 70.0f, 30.0f,
                                        3.0f * sidewaysSlack,
                                        damping);

                joints[(int)JointId.Neck].Initialise(limbs[(int)LimbId.Hips].PhysicsBody(),
                                        limbs[(int)LimbId.Head].PhysicsBody(),
                                        new Vector3(0.0f, 0.0f, 1.0f),
                                        new Vector3(-0.05f, 0.25f, 0.0f),
                                        haldWidth, 50.0f, 20.0f,
                                        3.0f * sidewaysSlack,
                                        damping);

                joints[(int)JointId.ShoulderLeft].Initialise(limbs[(int)LimbId.Torso].PhysicsBody(),
                                        limbs[(int)LimbId.UpperArmLeft].PhysicsBody(),
                                        new Vector3(1.0f, 0.0f, 0.0f),
                                        new Vector3(0.0f, 0.05f, 0.15f),
                                        haldWidth, 70.0f, 30.0f,
                                        0.7f,
                                        damping);

                joints[(int)JointId.ShoulderRight].Initialise(limbs[(int)LimbId.Torso].PhysicsBody(),
                                        limbs[(int)LimbId.UpperArmRight].PhysicsBody(),
                                        new Vector3(1.0f, 0.0f, 0.0f),
                                        new Vector3(0.0f, 0.05f, -0.15f),
                                        haldWidth, 30.0f, 75.0f,
                                        0.7f,
                                        damping);

                joints[(int)JointId.HipLeft].Initialise(limbs[(int)LimbId.Hips].PhysicsBody(),
                                        limbs[(int)LimbId.UpperLegLeft].PhysicsBody(),
                                        new Vector3(0.0f, 0.0f, 1.0f),
                                        new Vector3(0.0f, -0.25f, 0.12f),
                                        haldWidth, 10.0f, 60.0f,
                                        0.4f,
                                        damping);

                joints[(int)JointId.HipRight].Initialise(limbs[(int)LimbId.Hips].PhysicsBody(),
                                        limbs[(int)LimbId.UpperLegRight].PhysicsBody(),
                                        new Vector3(0.0f, 0.0f, 1.0f),
                                        new Vector3(0.0f, -0.25f, -0.12f),
                                        haldWidth, 10.0f, 60.0f,
                                        0.4f,
                                        damping);

                joints[(int)JointId.AnkleLeft].Initialise(limbs[(int)LimbId.LowerLegLeft].PhysicsBody(),
                                        limbs[(int)LimbId.FootLeft].PhysicsBody(),
                                        new Vector3(0.0f, 0.0f, 1.0f),
                                        new Vector3(0.0f, -0.15f, 0.15f),
                                        haldWidth, 30.0f, 10.0f,
                                        0.01f,
                                        damping);

                joints[(int)JointId.AnkleRight].Initialise(limbs[(int)LimbId.LowerLegRight].PhysicsBody(),
                                        limbs[(int)LimbId.FootRight].PhysicsBody(),
                                        new Vector3(0.0f, 0.0f, 1.0f),
                                        new Vector3(0.0f, -0.15f, -0.15f),
                                        haldWidth, 30.0f, 10.0f,
                                        0.01f,
                                         damping);

                joints[(int)JointId.WristLeft].Initialise(limbs[(int)LimbId.LowerArmLeft].PhysicsBody(),
                                        limbs[(int)LimbId.HandLeft].PhysicsBody(),
                                        new Vector3(0.0f, 1.0f, 0.0f),
                                        new Vector3(0.0f, 0.0f, 0.12f),
                                        haldWidth, 45.0f, 70.0f,
                                        0.01f,
                                        damping);

                joints[(int)JointId.WristRight].Initialise(limbs[(int)LimbId.LowerArmRight].PhysicsBody(),
                                        limbs[(int)LimbId.HandRight].PhysicsBody(),
                                        new Vector3(0.0f, 1.0f, 0.0f),
                                        new Vector3(0.0f, 0.0f, -0.12f),
                                        haldWidth, 45.0f, 70.0f,
                                        0.01f,
                                        damping);

            }
            else
            {
                joints[(int)JointId.Neck].Initialise(limbs[(int)LimbId.Torso].PhysicsBody(),
                              limbs[(int)LimbId.Head].PhysicsBody(),
                              new Vector3(0.0f, 0.0f, 1.0f),
                              new Vector3(-0.05f, 0.25f, 0.0f),
                              haldWidth, 50.0f, 20.0f,
                              3 * sidewaysSlack,
                              damping);

                joints[(int)JointId.ShoulderLeft].Initialise(limbs[(int)LimbId.Torso].PhysicsBody(),
                              limbs[(int)LimbId.UpperArmLeft].PhysicsBody(),
                              new Vector3(1.0f, 0.0f, 0.0f),
                              new Vector3(0.0f, 0.25f, 0.15f),
                              haldWidth, 30.0f, 75.0f,
                              0.7f,
                              damping);

                joints[(int)JointId.ShoulderRight].Initialise(limbs[(int)LimbId.Torso].PhysicsBody(),
                              limbs[(int)LimbId.UpperArmRight].PhysicsBody(),
                              new Vector3(1.0f, 0.0f, 0.0f),
                              new Vector3(0.0f, 0.25f, -0.15f),
                              haldWidth, 75.0f, 30.0f,
                              0.7f,
                              damping);


                joints[(int)JointId.HipLeft].Initialise(limbs[(int)LimbId.Torso].PhysicsBody(),
                              limbs[(int)LimbId.UpperLegLeft].PhysicsBody(),
                              new Vector3(0.0f, 0.0f, 1.0f),
                              new Vector3(0.0f, -0.25f, 0.12f),
                              haldWidth, 10.0f, 60.0f,
                              0.4f,
                              damping);


                joints[(int)JointId.HipRight].Initialise(limbs[(int)LimbId.Torso].PhysicsBody(),
                              limbs[(int)LimbId.UpperLegRight].PhysicsBody(),
                              new Vector3(0.0f, 0.0f, 1.0f),
                              new Vector3(0.0f, -0.25f, -0.12f),
                              haldWidth, 10.0f, 60.0f,
                              0.4f,
                              damping);


            }


            joints[(int)JointId.KneeLeft].Initialise(limbs[(int)LimbId.UpperLegLeft].PhysicsBody(),
                        limbs[(int)LimbId.LowerLegLeft].PhysicsBody(),
                        new Vector3(0.0f, 0.0f, 1.0f),
                        new Vector3(0.0f, -0.15f, 0.0f),
                        haldWidth, 100.0f, 0.0f,
                        sidewaysSlack,
                        damping);

            joints[(int)JointId.KneeRight].Initialise(limbs[(int)LimbId.UpperLegRight].PhysicsBody(),
                        limbs[(int)LimbId.LowerLegRight].PhysicsBody(),
                        new Vector3(0.0f, 0.0f, 1.0f),
                        new Vector3(0.0f, -0.15f, 0.0f),
                        haldWidth, 100.0f, 0.0f,
                        sidewaysSlack,
                        damping);

            joints[(int)JointId.ElbowLeft].Initialise(limbs[(int)LimbId.UpperArmLeft].PhysicsBody(),
                        limbs[(int)LimbId.LowerArmLeft].PhysicsBody(),
                        new Vector3(0.0f, 0.0f, 1.0f),
                        new Vector3(0.0f, 0.0f, 0.13f),
                        haldWidth, 0.0f, 130.0f,
                        sidewaysSlack,
                        damping);

            joints[(int)JointId.ElbowRight].Initialise(limbs[(int)LimbId.UpperArmRight].PhysicsBody(),
                        limbs[(int)LimbId.LowerArmRight].PhysicsBody(),
                        new Vector3(0.0f, 0.0f, 1.0f),
                        new Vector3(0.0f, 0.0f, -0.13f),
                        haldWidth, 130.0f, 0.0f,
                        sidewaysSlack,
                        damping);



            // disable some collisions between adjacent pairs
            DisableCollisions(limbs[(int)LimbId.Torso].PhysicsBody(), limbs[(int)LimbId.Head].PhysicsBody());
            DisableCollisions(limbs[(int)LimbId.Torso].PhysicsBody(), limbs[(int)LimbId.UpperLegLeft].PhysicsBody());
            DisableCollisions(limbs[(int)LimbId.Torso].PhysicsBody(), limbs[(int)LimbId.UpperLegRight].PhysicsBody());
            DisableCollisions(limbs[(int)LimbId.Torso].PhysicsBody(), limbs[(int)LimbId.UpperArmLeft].PhysicsBody());
            DisableCollisions(limbs[(int)LimbId.Torso].PhysicsBody(), limbs[(int)LimbId.UpperArmRight].PhysicsBody());
            DisableCollisions(limbs[(int)LimbId.UpperLegLeft].PhysicsBody(), limbs[(int)LimbId.LowerLegLeft].PhysicsBody());
            DisableCollisions(limbs[(int)LimbId.UpperLegRight].PhysicsBody(), limbs[(int)LimbId.LowerLegRight].PhysicsBody());
            DisableCollisions(limbs[(int)LimbId.UpperArmLeft].PhysicsBody(), limbs[(int)LimbId.LowerArmLeft].PhysicsBody());
            DisableCollisions(limbs[(int)LimbId.UpperArmRight].PhysicsBody(), limbs[(int)LimbId.LowerArmRight].PhysicsBody());

            if (type == RagdollType.Complex)
            {
                DisableCollisions(limbs[(int)LimbId.Torso].PhysicsBody(), limbs[(int)LimbId.Hips].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.LowerLegLeft].PhysicsBody(), limbs[(int)LimbId.FootLeft].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.LowerLegRight].PhysicsBody(), limbs[(int)LimbId.FootRight].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.LowerArmLeft].PhysicsBody(), limbs[(int)LimbId.HandLeft].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.LowerArmRight].PhysicsBody(), limbs[(int)LimbId.HandRight].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.Hips].PhysicsBody(), limbs[(int)LimbId.UpperLegLeft].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.Hips].PhysicsBody(), limbs[(int)LimbId.UpperLegRight].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.Hips].PhysicsBody(), limbs[(int)LimbId.UpperArmLeft].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.Hips].PhysicsBody(), limbs[(int)LimbId.UpperArmRight].PhysicsBody());
            }

            // he's not double-jointed...
            DisableCollisions(limbs[(int)LimbId.Torso].PhysicsBody(), limbs[(int)LimbId.LowerLegLeft].PhysicsBody());
            DisableCollisions(limbs[(int)LimbId.Torso].PhysicsBody(), limbs[(int)LimbId.LowerLegRight].PhysicsBody());
            DisableCollisions(limbs[(int)LimbId.UpperArmLeft].PhysicsBody(), limbs[(int)LimbId.UpperArmRight].PhysicsBody());
            DisableCollisions(limbs[(int)LimbId.UpperArmRight].PhysicsBody(), limbs[(int)LimbId.UpperArmLeft].PhysicsBody());
            DisableCollisions(limbs[(int)LimbId.UpperArmLeft].PhysicsBody(), limbs[(int)LimbId.Head].PhysicsBody());
            DisableCollisions(limbs[(int)LimbId.UpperArmRight].PhysicsBody(), limbs[(int)LimbId.Head].PhysicsBody());
            DisableCollisions(limbs[(int)LimbId.UpperLegLeft].PhysicsBody(), limbs[(int)LimbId.Head].PhysicsBody());
            DisableCollisions(limbs[(int)LimbId.UpperLegRight].PhysicsBody(), limbs[(int)LimbId.Head].PhysicsBody());
            DisableCollisions(limbs[(int)LimbId.LowerLegLeft].PhysicsBody(), limbs[(int)LimbId.Head].PhysicsBody());
            DisableCollisions(limbs[(int)LimbId.LowerLegRight].PhysicsBody(), limbs[(int)LimbId.Head].PhysicsBody());
            DisableCollisions(limbs[(int)LimbId.LowerLegLeft].PhysicsBody(), limbs[(int)LimbId.Head].PhysicsBody());
            DisableCollisions(limbs[(int)LimbId.LowerLegRight].PhysicsBody(), limbs[(int)LimbId.Head].PhysicsBody());

            if (type == RagdollType.Complex)
            {
                DisableCollisions(limbs[(int)LimbId.Torso].PhysicsBody(), limbs[(int)LimbId.FootLeft].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.Torso].PhysicsBody(), limbs[(int)LimbId.FootRight].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.Hips].PhysicsBody(), limbs[(int)LimbId.FootLeft].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.Hips].PhysicsBody(), limbs[(int)LimbId.FootRight].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.Hips].PhysicsBody(), limbs[(int)LimbId.LowerLegLeft].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.Hips].PhysicsBody(), limbs[(int)LimbId.LowerLegRight].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.Hips].PhysicsBody(), limbs[(int)LimbId.LowerArmLeft].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.Hips].PhysicsBody(), limbs[(int)LimbId.LowerArmRight].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.FootLeft].PhysicsBody(), limbs[(int)LimbId.HandLeft].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.FootRight].PhysicsBody(), limbs[(int)LimbId.HandRight].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.FootLeft].PhysicsBody(), limbs[(int)LimbId.Head].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.FootRight].PhysicsBody(), limbs[(int)LimbId.Head].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.FootLeft].PhysicsBody(), limbs[(int)LimbId.UpperArmLeft].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.FootRight].PhysicsBody(), limbs[(int)LimbId.UpperArmRight].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.FootLeft].PhysicsBody(), limbs[(int)LimbId.UpperArmRight].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.FootRight].PhysicsBody(), limbs[(int)LimbId.UpperArmLeft].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.FootLeft].PhysicsBody(), limbs[(int)LimbId.UpperLegLeft].PhysicsBody());
                DisableCollisions(limbs[(int)LimbId.FootRight].PhysicsBody(), limbs[(int)LimbId.UpperLegRight].PhysicsBody());
            }

            foreach (HingeJoint joint in joints)
            {
                if (joint != null)
                    joint.EnableHinge();
            }

            foreach (IPhysicsObject limb in limbs)
            {
                if (limb != null)
                {
                    limb.PhysicsBody().CollisionSkin.SetMaterialProperties(0, new JigLibX.Collision.MaterialProperties(0.2f, 3.0f, 2.0f));
                    container.Children.Add(limb as Balder.INode);
                    physicsObjects.Add(limb);

                    if (limb is PhysicsCapsule)
                        (limb as PhysicsCapsule).Material = Materials.Yellow;
                    if (limb is PhysicsSphere)
                        (limb as PhysicsSphere).Material = Materials.Yellow;
                }


            }

            MoveTorso(Utils.VectorBalderToXna( position));
           
        }

        public void PutToSleep()
        {
            foreach (IPhysicsObject limb in limbs)
            {
                if (limb != null)
                    limb.PhysicsBody().SetInactive();
            } 
        }

        private void MoveTorso(Vector3 pos)
        {
            Vector3 delta = pos - limbs[(int)LimbId.Torso].PhysicsBody().Position;
            foreach (IPhysicsObject limb in limbs)
            {
                if (limb != null)
                {
                    Vector3 origPos = limb.PhysicsBody().Position;
                    limb.PhysicsBody().MoveTo(origPos + delta, limb.PhysicsBody().Orientation);
                }
            }
        }

        public Vector3 Position
        {
            set { MoveTorso(value); }
            get { return limbs[(int)LimbId.Torso].PhysicsBody().Position; }
        }

    }
}
